#!/usr/bin/env python
#-*- coding:utf-8   -*-
from __future__ import division
import os, sys
dir_mytest = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
sys.path.insert(0, dir_mytest)
import rospy
import time, math
import numpy as np
from sensor_msgs.msg import LaserScan
from std_msgs.msg import String
from geometry_msgs.msg import Twist
from scipy.signal import savgol_filter
from pm_message_msg.msg import Cell_old
from nav_msgs.msg import Odometry
from log.log import initLogConf



# 导航到充电目标点
# 机器人充电桩对接

# robot_server.py 中调用充电
class Obstacle():
    def __init__(self): 
        self.loger = initLogConf("auto_power")
        # self.sensor = rospy.Publisher("sensor_control", String, queue_size=1)
        self.liner_speed = 0.05  # 默认线速度
        self.anage_speed = 0.05  # 默认角速度
        self.turn_speed = 0.2
        self.rate_pub = 10   # 频率
        self.is_shutingdown = False  # 充电成功 退出程序,,默认为False
        self.dance = 0.40   # 充电桩厚度 0.42
        self.dance_out = 0.1 # 出站距离 0.1
        self.angle = 180 # 出站指定角度 度
        self.rate = rospy.Rate(self.rate_pub)

        self._cmd_pub = rospy.Publisher("cmd_vel", Twist, queue_size=1)

    def get_once(self):
        """接收激光雷达数据，并进行处理"""
        try:
            squares = rospy.wait_for_message("scan", LaserScan)
            squares = squares.ranges[220:500]
        except:
            self.shutdown()
        
        scan_data = list(savgol_filter(squares, 9, 3, mode="nearest"))


        # 获取特征点数据
        min_scan = min(scan_data) # 获取最小值
        min_index = scan_data.index(min_scan) # 获取最小值索引
        # 获取第二个谷值
        min_a = min(scan_data[0:min_index-5])
        min_b = min(scan_data[min_index+5:-1])
        if min_a > min_b:
            left_index = min_index + 5 + scan_data[min_index+5:-1].index(min_b)
            left_scan = min_b
            right_index = min_index
            right_scan = min_scan
            
        else:
            right_index = scan_data[0:min_index-5].index(min_a)
            right_scan = min_a
            left_index = min_index
            left_scan = min_scan
        
        mid_list = scan_data[right_index:left_index]
        mid_index = right_index + mid_list.index(max(mid_list))
        mid = len(scan_data) / 2
        
        twist = Twist()
        if abs(mid_index - mid) < 2:
            ali = True
            twist.linear.x = 0
            twist.angular.z = 0

        elif mid_index > mid:
            ali = False
            twist.linear.x = 0
            twist.angular.z = self.anage_speed

        elif mid_index < mid:
            ali = False
            twist.linear.x = 0
            twist.angular.z = -self.anage_speed
        self._cmd_pub.publish(twist)
        
        return ali
    
    def get_more(self):
        """接收激光雷达数据，并进行处理"""
        i = 0
        data_list = []
        # 获取20组数据的平均值
        while i < 30:
            try:
                receive = rospy.wait_for_message("scan", LaserScan)
                data_list.append(receive.ranges[220:500])
            except:
                self.shutdown()
            i+=1

        try:
            squares = rospy.wait_for_message("scan", LaserScan)
            squares = squares.ranges[220:500]
        except:
            self.shutdown()
        
        # scan_data = list(savgol_filter(squares, 9, 3, mode="nearest"))

        scan_data = []
        for one in range(len(data_list[0])):
            zero_list = []
            for zero in data_list:
                zero_list.append(zero[one])
            scan_data.append(np.mean(zero_list))
        

        # 获取特征点数据
        min_scan = min(scan_data) # 获取最小值
        min_index = scan_data.index(min_scan) # 获取最小值索引
        # 获取第二个谷值
        min_a = min(scan_data[0:min_index-5])
        min_b = min(scan_data[min_index+5:-1])
        if min_a > min_b:
            left_index = min_index + 5 + scan_data[min_index+5:-1].index(min_b)
            left_scan = min_b
            right_index = min_index
            right_scan = min_scan
        else:
            right_index = scan_data[0:min_index-5].index(min_a)
            right_scan = min_a
            left_index = min_index
            left_scan = min_scan
        
        
        mid_scan = max(scan_data[right_index:left_index])

        # 计算前进距离
        move_dance= mid_scan - self.dance
        return move_dance


    def status_power(self):
        """判断是否充上电"""
        try:
            get_msg = rospy.wait_for_message("cell_status", Cell_old)
        except:
            self.shutdown()

        power_status = False
        if get_msg.ele_status:
            power_status = True
        elif int(get_msg.voltage) > 25 and int(get_msg.ele) == 0 and int(get_msg.ratio) == 100:
            power_status = True
        
        # return power_status
        return True
    
    def twist_num(self, distance, liner_speed):
        """移动固定距离
        dist : 动态移动距离
        distance : 要移动的距离
        """
        self.loger.info("moving")
        receive = rospy.wait_for_message("/raw_odom", Odometry)
        start_x = receive.pose.pose.position.x

        while not rospy.is_shutdown():
            receive = rospy.wait_for_message("/raw_odom", Odometry)
            now_x = receive.pose.pose.position.x
            dist = 0
            if start_x >= 0 and now_x >= 0:
                dist = abs(now_x - start_x)
            elif start_x >= 0 and now_x <= 0:
                dist = abs(start_x - now_x)
            elif start_x < 0 and now_x <= 0:
                dist = abs(abs(now_x) - abs(start_x))
            elif start_x < 0 and now_x >= 0:
                dist = abs(now_x - start_x)
            print("dist: ", dist, start_x, now_x)

            if dist >= distance:
                twist = Twist()
                twist.linear.x = 0
                twist.linear.y = 0
                twist.angular.z = 0
                self._cmd_pub.publish(twist)
                self.loger.info("break")
                break
            else:
                twist = Twist()
                twist.linear.x = liner_speed
                twist.angular.z = 0
                self._cmd_pub.publish(twist)
                self.rate.sleep()

        return



    def move_in(self):
        """底盘控制，自动对接充电
        """
        # 先关闭避障
        # msg = String()
        # msg.data = "0021"
        # self.sensor.publish(msg)

        while not rospy.is_shutdown():
            if self.get_once():
                break

        move_dance = self.get_more()
        self.loger.info(move_dance)
        self.twist_num(move_dance/2, self.liner_speed)
        while not rospy.is_shutdown():
            if self.get_once():
                break
        self.twist_num(move_dance/2, self.liner_speed)
        return True
    
    def charging(self):
        """执行充电程序"""
        change_status = False
        for one in range(5):
            self.move_in()
            time.sleep(2)
            status = self.status_power()
            if status:
                change_status = True
                break
            else:
                self.twist_num(0.01, 0.05)
                time.sleep(2)
                status = self.status_power()
                if status:
                    change_status = True
                    break
                else:
                    self.twist_num(0.3, -0.1)
        return change_status

    def move_turn(self, angle):
        """原地旋转"""
        ang_speed = 0.3
        time_num = angle/180*math.pi/ang_speed
        start_time = time.time()
        twist = Twist()
        while not rospy.is_shutdown():
            if time.time()- start_time >= time_num:
                twist.linear.x = 0
                twist.angular.z = 0
                self._cmd_pub.publish(twist)
                break
            twist.linear.x = 0
            twist.angular.z = -ang_speed
            self._cmd_pub.publish(twist)
            self.rate.sleep()
        return True
    
    def move_out(self):
        """驶出充电区
        """
        self.twist_num(self.dance_out, -0.1)
        self.move_turn(170)
        return True

    

    def shutdown(self):
        twist = Twist()
        twist.linear.x = 0
        twist.angular.z = 0
        self._cmd_pub.publish(twist)
        self.loger.info("shotdown")
        pass



if __name__ == '__main__':
    rospy.init_node('move_obstacle')
    obstacle = Obstacle()
    obstacle.move_turn(90)
    # print(obstacle.status_power())

